/*
*	Brian Bousquet-Smith  Chaz Godwin
*	OBDII Functions
*	Purpose: The purpose of these functions is to directly interact with the OBDII port of a car.
*			Functions are designed to connect via serial to an ELM327 1.4b microprocessor. This 
*			chip is designed to interpret the data signals from the vehicle, and respond with ascii
*			characters. 
*/



#include "obd.h"
#include <avr/io.h>
#include <avr/iom164.h>
#include <stdlib.h>

int elm_init(){

/*
*	Purpose: Initialize ELM and check that communication is correct
*	Input: Nothing
*	Output:	Returns OK or corresponding error codes
*/
	int error;
	unsigned char in_buff[3] = {'A', 'T', 'Z'};
	unsigned char exp_buff[15] = {'A', 'T', 'Z', 'E', 'L', 'M', '3', '2', '7', 'v', '1', '.', '4', 'b', 0x3E};
	unsigned char out_buff[100];		
	
	// Get the response from the ELM 
	error = obd_sendcmd(in_buff, out_buff, sizeof(in_buff));	
	if(error == OVERFLOW){
		return error;
	}
	
	// Check that data is as expected
	error = check_data(out_buff, exp_buff, sizeof(exp_buff));
	if(error == BAD_DATA){
		return error;
	}
	return OK;	
}

int obd_init(){

/*
*    Purpose: Initialize OBDII port and check that communication is correct
*    Input: Nothing
*    Output: Returns OK if successful otherwises returns corresponding error codes
*/
    
    unsigned char in_buff[4] = {'0', '1', '0', '0'};
    unsigned char exp_buff1[31] = "0100SEARCHING...UNABLETOCONNECT";
	unsigned char exp_buff2[20] = "0100SEARCHING...4100";
	unsigned char exp_buff3[8] = "01004100";
	unsigned char out_buff[100];
	int error = 0;

	// Send command to ELM

	error = obd_sendcmd(in_buff, out_buff, sizeof(in_buff));
	if(error == OVERFLOW){
		return error;
	}

	// Find protocol	

	error = check_data(out_buff, exp_buff1, sizeof(exp_buff1));
	if(error == BAD_DATA){	// Means something other than unable to connect was seen
		error = check_data(out_buff, exp_buff2, sizeof(exp_buff2));
		if (error == BAD_DATA){	// Means didn't match correctly with expected buff 2
			error = check_data(out_buff, exp_buff3, sizeof(exp_buff3));
			if(error == BAD_DATA){ // Didn't match any of the expected buffer, must be erro
				return BAD_DATA;
			}
			return OK;	// Matched 3rd expected buffer so its ready to get data
		}
		return OK; // Matched 2nd expected buffer so its ready to get data
	}
	return NO_DEVICE_FOUND;	
}

void obd_transmit (unsigned char data){

/*
*	Purpose: Transmit data to the LCD
*	Input: Data to be transmitted
*	Output:	Nothing
*/

	while( !(UCSR0A & (1<<UDRE0)));
	UDR0 = data;
}

unsigned char obd_receive (void){

/*
*	Purpose: Receive data from the LCD
*	Input: Nothing
*	Output:	Data received	
*/
	while (!(UCSR0A & (1<<RXC0)));
	return UDR0;
}

void obd_flush(void) {

/*
*	Purpose: Receive data from the LCD
*	Input: Nothing
*	Output:	Nothing	
*/

	char tmp;
	while( UCSR0A & (1<<RXC1)) tmp = UDR0;
}

int obd_sendcmd(unsigned char *in, unsigned char *out, char len_in){

/*
*	Purpose: Send a command to ELM to run
*	Input:	in 	- Command buffer		
*		 	out 	- Location to output results
*		 	len_in - Length of the input
*	Output: Returns OK or corresponding error codes
*/		

	//TODO Start Interrupt
	int i;
	obd_flush();			// Kill all data in the obd buffer
	
	// Transmit Command to the ELM

	for(i=0;i<len_in;i++){		
		obd_transmit(in[i]);	
	}
	obd_transmit(COMMAND_END); //End byte for the command
	// Output Response in Buffer
	for(i=0;i<100;i++){
		out[i] = obd_receive();
		if (out[i] == RESPONSE_END){
			return OK; // When end is reached, return
		}
	}
	return OVERFLOW;
}

int check_data(unsigned char *resp, unsigned char *exp, int len_exp){

/*
*	Purpose: Check to make sure the data is as expected
*	Input:	resp 	- Response from the ELM/OBDII
*			exp		- Expected response
*			len_exp	- Length of expected response
*	Output: Returns OK or corresponding error codes
*/
	int i;
	int j = 0;
	for(i=0;i<len_exp;i++){	
		while( (resp[j]=='\n') || (resp[j]=='\r') || (resp[j]==' ') || resp[j]=='\0') j++; // Ignore all new lines/carriage returns/spaces and null bytes
		if(resp[j]!=exp[i]){
			// Check to make sure data is what is expected
			return BAD_DATA;	// If not return
		}
		j++;	
	}	
	return j;
}

int record_data(unsigned char *in, unsigned char *out, int offset, int len_in){

/*
*	Purpose: To record the desired data from the string
*	Input: 	in 	- buffer getting data from
*		out 	- buffer outputing data to  
*		offset 	- offset from the input buffer where data begins
*		len_in	- length of input buffer
*/

	int i;
	for(i=0;i<len_in;i++){
		while( (in[offset]=='\n') || (in[offset]=='\r') || (in[offset]==' ')) offset++;
		if(offset >= 100) return OVERFLOW;
		out[i] = in[offset];
		offset++;
	}
	out[i] = '\0'; 
	return OK;
}	

int get_mph(void){

/*
*    Purpose: Get MPH Data and Output it
*    Input: Nothing
*    Output: Mph of the vehicle or an error code, refer to error codes for their meaning
*/
    static int MPH_MAX = 128;
    static int MPH_MIN = 0;
    int error = 0, count;
    unsigned int mph;
    unsigned char in_buff[4] = {'0', '1', '0', 'D'}; // Input Command Buffer
    unsigned char exp_buff[8] = {'0', '1', '0', 'D', '4', '1', '0', 'D'}; // Expected Data Buffer
    unsigned char out_buff[100];    // Buffer to fill with data 
    unsigned char mph_buff[3];     // Buffer to fill with actual MPH

    // Send command and get data back from ELM/OBDII

    error = obd_sendcmd(in_buff, out_buff, 4); // Send command
    if(error == OVERFLOW){
        return error;
    }
    count = check_data(out_buff, exp_buff, sizeof(exp_buff));
    if(count < 0){
        return BAD_DATA;
    }
    // Record MPH Data
    error = record_data(out_buff, mph_buff, count, sizeof(mph_buff)-1);
    if(error == OVERFLOW){
        return error;
    }
    // Convert to MPH

    mph =(unsigned int)strtol((char *)mph_buff, NULL, 16);
    mph = mph*.621371;    // Converts KMPH to MPH    
    
    // Variable size based on how big mph is
    if(mph>MPH_MAX||mph<MPH_MIN){    // MPH is less than 0 or greater than 128...both impossible
        return IMP_DATA;
    }
    return mph;
}


int get_rpm(void){

/*
*    Purpose: Get RPM Data and Output it
*    Input: Nothing
*    Output: Outputs the rpm of the car or an error code, refer to error codes for their meaning
*/
    static int RPM_MAX = 16383;
    static int RPM_MIN = 0;
    int error = 0, count = 0;
    unsigned int rpm;
    unsigned char in_buff[4] = {'0', '1', '0', 'C'};    
    unsigned char exp_buff[8] = {'0', '1', '0', 'C', '4', '1', '0', 'C'};     
    unsigned char out_buff[100];
    unsigned char rpm_buff[5]; 
    error = obd_sendcmd(in_buff, out_buff, 4);
    if(error == OVERFLOW){
        return error;
    }
    count = check_data(out_buff, exp_buff, sizeof(exp_buff));
    if(count<0){
        return BAD_DATA;
    }
    error = record_data(out_buff, rpm_buff, count, sizeof(rpm_buff)-1);
    if(error == OVERFLOW){
        return error;
    }

    rpm =(unsigned int)strtol((char *)rpm_buff, NULL, 16);    
    rpm = rpm/4;

    if(rpm>RPM_MAX||rpm<RPM_MIN){    // MPH is less than 0 or greater than 128...both impossible
        return IMP_DATA;
    }

    return rpm;
}


